#ifndef _LEAST_SQUARE_H
#define _LEAST_SQUARE_H

#include <stdlib.h>
#include <math.h>

/* 3 by 3 matrix inverse */
void Inv33(double *src, double *dst)
{
	double p1 = src[0] * (src[4] * src[8] - src[5] * src[7]);
	double p2 = src[1] * (src[3] * src[8] - src[5] * src[6]);
	double p3 = src[2] * (src[3] * src[7] - src[4] * src[6]);

	double det = p1 - p2 + p3;

	double t[9];
	t[0] = src[0]; t[1] = src[3]; t[2] = src[6];
	t[3] = src[1]; t[4] = src[4]; t[5] = src[7];
	t[6] = src[2]; t[7] = src[5]; t[8] = src[8];

	dst[0] = (t[4] * t[8] - t[5] * t[7]) / det * 1.0;
	dst[1] = (t[3] * t[8] - t[5] * t[6]) / det * -1.0;
	dst[2] = (t[3] * t[7] - t[4] * t[6]) / det * 1.0;

	dst[3] = (t[1] * t[8] - t[2] * t[7]) / det * -1.0;
	dst[4] = (t[0] * t[8] - t[2] * t[6]) / det * 1.0;
	dst[5] = (t[0] * t[7] - t[1] * t[6]) / det * -1.0;

	dst[6] = (t[1] * t[5] - t[2] * t[4]) / det * 1.0;
	dst[7] = (t[0] * t[5] - t[2] * t[3]) / det * -1.0;
	dst[8] = (t[0] * t[4] - t[1] * t[3]) / det * 1.0;
}

/* 4 by 4 matrix inverse */
void Inv44(double *a, double *dst)
{
	double b11 = a[5] * a[10] * a[15] + a[6] * a[11] * a[13] + a[7] * a[ 9] * a[14] - 
				 a[5] * a[11] * a[14] - a[6] * a[ 9] * a[15] - a[7] * a[10] * a[13];
	double b12 = a[1] * a[11] * a[14] + a[2] * a[ 9] * a[15] + a[3] * a[10] * a[13] - 
				 a[1] * a[10] * a[15] - a[2] * a[11] * a[13] - a[3] * a[ 9] * a[14];
	double b13 = a[1] * a[ 6] * a[15] + a[2] * a[ 7] * a[13] + a[3] * a[ 5] * a[14] - 
				 a[1] * a[ 7] * a[14] - a[2] * a[ 5] * a[15] - a[3] * a[ 6] * a[13];
	double b14 = a[1] * a[ 7] * a[10] + a[2] * a[ 5] * a[11] + a[3] * a[ 6] * a[ 9] - 
				 a[1] * a[ 6] * a[11] - a[2] * a[ 7] * a[ 9] - a[3] * a[ 5] * a[10];

	double b21 = a[4] * a[11] * a[14] + a[6] * a[ 8] * a[15] + a[7] * a[10] * a[12] - 
				 a[4] * a[10] * a[15] - a[6] * a[11] * a[12] - a[7] * a[ 8] * a[14];
	double b22 = a[0] * a[10] * a[15] + a[2] * a[11] * a[12] + a[3] * a[ 8] * a[14] - 
				 a[0] * a[11] * a[14] - a[2] * a[ 8] * a[15] - a[3] * a[10] * a[12];
	double b23 = a[0] * a[ 7] * a[14] + a[2] * a[ 4] * a[15] + a[3] * a[ 6] * a[12] - 
				 a[0] * a[ 6] * a[15] - a[2] * a[ 7] * a[12] - a[3] * a[ 4] * a[14];
	double b24 = a[0] * a[ 6] * a[11] + a[2] * a[ 7] * a[ 8] + a[3] * a[ 4] * a[10] - 
				 a[0] * a[ 7] * a[10] - a[2] * a[ 4] * a[11] - a[3] * a[ 6] * a[ 8];

	double b31 = a[4] * a[ 9] * a[15] + a[5] * a[11] * a[12] + a[7] * a[8] * a[13] - 
				 a[4] * a[11] * a[13] - a[5] * a[ 8] * a[15] - a[7] * a[9] * a[12];
	double b32 = a[0] * a[11] * a[13] + a[1] * a[ 8] * a[15] + a[3] * a[9] * a[12] - 
				 a[0] * a[ 9] * a[15] - a[1] * a[11] * a[12] - a[3] * a[8] * a[13];
	double b33 = a[0] * a[ 5] * a[15] + a[1] * a[ 7] * a[12] + a[3] * a[4] * a[13] - 
				 a[0] * a[ 7] * a[13] - a[1] * a[ 4] * a[15] - a[3] * a[5] * a[12];
	double b34 = a[0] * a[ 7] * a[ 9] + a[1] * a[ 4] * a[11] + a[3] * a[5] * a[ 8] - 
				 a[0] * a[ 5] * a[11] - a[1] * a[ 7] * a[ 8] - a[3] * a[4] * a[ 9];

	double b41 = a[4] * a[10] * a[13] + a[5] * a[ 8] * a[14] + a[6] * a[9] * a[12] - 
				 a[4] * a[ 9] * a[14] - a[5] * a[10] * a[12] - a[6] * a[8] * a[13];
	double b42 = a[0] * a[ 9] * a[14] + a[1] * a[10] * a[12] + a[2] * a[8] * a[13] - 
				 a[0] * a[10] * a[13] - a[1] * a[ 8] * a[14] - a[2] * a[9] * a[12];
	double b43 = a[0] * a[ 6] * a[13] + a[1] * a[ 4] * a[14] + a[2] * a[5] * a[12] - 
				 a[0] * a[ 5] * a[14] - a[1] * a[ 6] * a[12] - a[2] * a[4] * a[13];
	double b44 = a[0] * a[ 5] * a[10] + a[1] * a[ 6] * a[ 8] + a[2] * a[4] * a[ 9] - 
				 a[0] * a[ 6] * a[ 9] - a[1] * a[ 4] * a[10] - a[2] * a[5] * a[ 8];

	double det = a[0] * b11 + a[1] * b21 + a[2] * b31 + a[3] * b41;

	dst[0] = b11 / det;
	dst[1] = b12 / det;
	dst[2] = b13 / det;
	dst[3] = b14 / det;

	dst[4] = b21 / det;
	dst[5] = b22 / det;
	dst[6] = b23 / det;
	dst[7] = b24 / det;

	dst[ 8] = b31 / det;
	dst[ 9] = b32 / det;
	dst[10] = b33 / det;
	dst[11] = b34 / det;

	dst[12] = b41 / det;
	dst[13] = b42 / det;
	dst[14] = b43 / det;
	dst[15] = b44 / det;
}

void LeastSquare3(float *A, float *x, float *y, const int n, const int m)
{
	if (m != 3)
		return;

	double *At = new double[n*m];
	for (int i = 0; i < n; ++i) {
		for (int j = 0; j < m; ++j) {
			At[j*n+i] = A[i*m+j];
		}
	}

	double *AA = new double[m*m];
	for (int i = 0; i < m; ++i) {
		for (int j = 0; j < m; ++j) {
			AA[i*m+j] = 0.0;
			for (int k = 0; k < n; ++k) {
				AA[i*m+j] += At[i*n+k] * A[k*m+j];
			}
		}
	}

	double *invAA = new double[m*m];
	Inv33(AA, invAA);

	double *AAA = new double[m*n];
	for (int i = 0; i < m; ++i) {
		for (int j = 0; j < n; ++j) {
			AAA[i*n+j] = 0.0f;
			for (int k = 0; k < m; ++k) {
				AAA[i*n+j] += invAA[i*m+k] * At[k*n+j];
			}
		}
	}

	for (int i = 0; i < m; ++i) {
		x[i] = 0.0f;
		for (int j = 0; j < n; ++j) {
			x[i] += (float)(AAA[i*n+j]) * y[j];
		}
	}

	delete[] At;
	delete[] AA;
	delete[] invAA;
	delete[] AAA;
}

void PseudoInverse4(double *src, double *dst, const int n, const int m)
{
	if (m != 4)
		return;

	double *At = new double[n*m];
	for (int i = 0; i < n; ++i) {
		for (int j = 0; j < m; ++j) {
			At[j*n+i] = src[i*m+j];
		}
	}

	double *AA = new double[m*m];
	for (int i = 0; i < m; ++i) {
		for (int j = 0; j < m; ++j) {
			AA[i*m+j] = 0.0;
			for (int k = 0; k < n; ++k) {
				AA[i*m+j] += At[i*n+k] * src[k*m+j];
			}
		}
	}

	double *invAA = new double[m*m];
	Inv44(AA, invAA);

	for (int i = 0; i < m; ++i) {
		for (int j = 0; j < n; ++j) {
			dst[i*n+j] = 0.0;
			for (int k = 0; k < m; ++k) {
				dst[i*n+j] += invAA[i*m+k] * At[k*n+j];
			}
		}
	}

	delete[] At;
	delete[] AA;
	delete[] invAA;
}

void LeastSquareFast4(double *pinvA, float *x, float *y, const int n, const int m)
{
	if (m != 4)
		return;

	for (int i = 0; i < m; ++i) {
		x[i] = 0.0f;
		for (int j = 0; j < n; ++j) {
			x[i] += (float)(pinvA[i*n+j]) * y[j];
		}
	}
}


void LeastSquare4(float *A, float *x, float *y, const int n, const int m)
{
	if (m != 4)
		return;

	double *At = new double[n*m];
	for (int i = 0; i < n; ++i) {
		for (int j = 0; j < m; ++j) {
			At[j*n+i] = A[i*m+j];
		}
	}

	double *AA = new double[m*m];
	for (int i = 0; i < m; ++i) {
		for (int j = 0; j < m; ++j) {
			AA[i*m+j] = 0.0;
			for (int k = 0; k < n; ++k) {
				AA[i*m+j] += At[i*n+k] * A[k*m+j];
			}
		}
	}

	double *invAA = new double[m*m];
	Inv44(AA, invAA);

	double *AAA = new double[m*n];
	for (int i = 0; i < m; ++i) {
		for (int j = 0; j < n; ++j) {
			AAA[i*n+j] = 0.0;
			for (int k = 0; k < m; ++k) {
				AAA[i*n+j] += invAA[i*m+k] * At[k*n+j];
			}
		}
	}

	for (int i = 0; i < m; ++i) {
		x[i] = 0.0f;
		for (int j = 0; j < n; ++j) {
			x[i] += (float)(AAA[i*n+j]) * y[j];
		}
	}

	delete[] At;
	delete[] AA;
	delete[] invAA;
	delete[] AAA;
}

#endif